#include "../_includes.h"

#define RED 1
#define BLUE 2
bool autochain = false;
bool isjudge = false;
bool istoarm = false;
int mycolor = 1;
int yourcolor = 1;

void testcolor()
{
    if (isjudge)
    {
        if (vis.takeSnapshot(yourcolor) && vis.largestObject.width > 200 && vis.largestObject.height > 100)
        {
            autochain = true;
            vis.setLedColor(255, 0, 0);
            // chain.stop();
            // this_thread::sleep_for(100);
            int startcode = mtr_chain.position(rotationUnits::deg) / 1300;
            int aimcode = startcode % 3 == 1 ? startcode * 1300 + 1260 : startcode * 1300 + 1320;
            while (mtr_chain.position(rotationUnits::deg) < aimcode)
            {
                mtr_chain.spin(directionType::fwd, 100, velocityUnits::pct);
            }
            // chain.spinTo(aimcode, rotationUnits::deg,100,velocityUnits::pct,true);
            chain.stop();
            this_thread::sleep_for(200);
        }
        else
        {
            autochain = false;
            vis.setLedColor(255, 255, 255);
        }
    }
}

void toarm()
{
    if (istoarm)
    {
        if (vis.takeSnapshot(mycolor) && vis.largestObject.width > 200 && vis.largestObject.height > 100)
        {
            autochain = true;
            vis.setLedColor(255, 0, 0);
            // chain.stop();
            // this_thread::sleep_for(100);
            int startcode = mtr_chain.position(rotationUnits::deg) / 1300;
            int aimcode = startcode * 1300 + 1000;
            while (mtr_chain.position(rotationUnits::deg) < aimcode)
            {
                mtr_chain.spin(directionType::fwd, 100, velocityUnits::pct);
            }
            // chain.spinTo(aimcode, rotationUnits::deg,100,velocityUnits::pct,true);
            chain.stop();
            this_thread::sleep_for(100);
            aimcode = startcode * 1300 - 1000;
            while (mtr_chain.position(rotationUnits::deg) > aimcode)
            {
                mtr_chain.spin(directionType::fwd, -100, velocityUnits::pct);
            }
            chain.stop();
            this_thread::sleep_for(100);
        }
        else
        {
            autochain = false;
            vis.setLedColor(255, 255, 255);
        }
    }
}

void usecolor()
{
    while (1)
    {
        testcolor();
        this_thread::sleep_for(20);
    }
}

void usetoarm()
{
    while (1)
    {
        toarm();
        this_thread::sleep_for(20);
    }
}

void getstate()
{
    if (ctrl.ButtonL1.pressing())
    {
        istoarm = true;
    }
    else
    {
        istoarm = false;
    }

    if (ctrl.ButtonL2.pressing())
    {
        isjudge = true;
    }
    else
    {
        isjudge = false;
    }

    if (!isjudge && !istoarm)
    {
        autochain = false;
    }
}

void changecolor()
{
    if (auto_info.alliance == Alliance::red)
    {
        mycolor = RED;
        yourcolor = BLUE;
    }
    else
    {
        mycolor = BLUE;
        yourcolor = RED;
    }
}